#include "Precompile.h"
#include "AnimationClip.h"

namespace Graphics{
	AnimationClip::AnimationClip(std::vector<unsigned int>& samplerIds, float startTime, float endTime, std::string &name):
		m_name(name),m_start(startTime),m_end(endTime),m_samplerIds(samplerIds){
	}

	void AnimationClip::computeClipJointFrame(float time, std::vector<Utility::Math::Matrix<4,4>>& joints, std::vector<Graphics::SkeletonJointPtr>& skelJoints, RenderContext* context){
		std::map<float,AnimationFramePtr>::iterator itr;
		if((itr = m_savedFrames.find(time)) != m_savedFrames.end()){
			joints = itr->second->m_precomputedMatrices;
			return;
		}

		// Create frame
		AnimationFramePtr frame = AnimationFramePtr(new AnimationFrame);
		m_savedFrames[time] = frame;
		
		std::vector<Utility::Math::Vector<float,3>> translate, scale, rotationAngle, rotationAngleAnim;
		std::vector<Utility::Math::Matrix<4,4>> transforms;

		joints.clear();
		extractSamplerInformation(time,m_samplerIds,frame->m_translate,frame->m_scale,frame->m_rotationAngle,frame->m_rotationAngleAnim,frame->m_transforms,skelJoints,context);

		composeMatrices(frame->m_translate,frame->m_scale,frame->m_rotationAngle,frame->m_rotationAngleAnim,frame->m_transforms,skelJoints,joints);
		frame->m_precomputedMatrices = joints;
	}

	void AnimationClip::computeClipJointFrame(float sourceTime, float targetTime, float interpolate, AnimationClipPtr targetClip, std::vector<Utility::Math::Matrix<4,4>>& joints, std::vector<Graphics::SkeletonJointPtr>& skelJoints, RenderContext* context){
		AnimationFramePtr sourceFrame,targetFrame;
		sourceFrame = getAnimationFrame(sourceTime);
		targetFrame = targetClip->getAnimationFrame(targetTime);

		// Compute frames if not yet computed
		if(!sourceFrame){
			computeClipJointFrame(sourceTime,joints,skelJoints,context);
			sourceFrame = getAnimationFrame(sourceTime);
		}
		if(!targetFrame){
			targetClip->computeClipJointFrame(targetTime,joints,skelJoints,context);
			targetFrame = targetClip->getAnimationFrame(targetTime);
		}

		if(!targetFrame || !sourceFrame){
			LOG_ERROR("Source or target frame missing, this should not be possible!");
			assert(0);
			return;
		}

		std::vector<Utility::Math::Vector<float,3>> translate, scale, rotationAngle, rotationAngleAnim;
		std::vector<Utility::Math::Matrix<4,4>> transforms;
		// Interpolate between results
		if(sourceFrame->m_transforms.empty()){
			Utility::Math::Vector<> tmp;
			for(unsigned int i=0; i<sourceFrame->m_translate.size(); i++){
				tmp(Utility::Math::_x_,(sourceFrame->m_translate[i](Utility::Math::_x_)*(1-interpolate)+targetFrame->m_translate[i](Utility::Math::_x_)*interpolate));
				tmp(Utility::Math::_y_,(sourceFrame->m_translate[i](Utility::Math::_y_)*(1-interpolate)+targetFrame->m_translate[i](Utility::Math::_y_)*interpolate));
				tmp(Utility::Math::_z_,(sourceFrame->m_translate[i](Utility::Math::_z_)*(1-interpolate)+targetFrame->m_translate[i](Utility::Math::_z_)*interpolate));
				translate.push_back(tmp);

				tmp(Utility::Math::_x_,(sourceFrame->m_scale[i](Utility::Math::_x_)*(1-interpolate)+targetFrame->m_scale[i](Utility::Math::_x_)*interpolate));
				tmp(Utility::Math::_y_,(sourceFrame->m_scale[i](Utility::Math::_y_)*(1-interpolate)+targetFrame->m_scale[i](Utility::Math::_y_)*interpolate));
				tmp(Utility::Math::_z_,(sourceFrame->m_scale[i](Utility::Math::_z_)*(1-interpolate)+targetFrame->m_scale[i](Utility::Math::_z_)*interpolate));
				scale.push_back(tmp);
				
				tmp(Utility::Math::_x_,(sourceFrame->m_rotationAngle[i](Utility::Math::_x_)*(1-interpolate)+targetFrame->m_rotationAngle[i](Utility::Math::_x_)*interpolate));
				tmp(Utility::Math::_y_,(sourceFrame->m_rotationAngle[i](Utility::Math::_y_)*(1-interpolate)+targetFrame->m_rotationAngle[i](Utility::Math::_y_)*interpolate));
				tmp(Utility::Math::_z_,(sourceFrame->m_rotationAngle[i](Utility::Math::_z_)*(1-interpolate)+targetFrame->m_rotationAngle[i](Utility::Math::_z_)*interpolate));
				rotationAngle.push_back(tmp);
				
				tmp(Utility::Math::_x_,(sourceFrame->m_rotationAngleAnim[i](Utility::Math::_x_)*(1-interpolate)+targetFrame->m_rotationAngleAnim[i](Utility::Math::_x_)*interpolate));
				tmp(Utility::Math::_y_,(sourceFrame->m_rotationAngleAnim[i](Utility::Math::_y_)*(1-interpolate)+targetFrame->m_rotationAngleAnim[i](Utility::Math::_y_)*interpolate));
				tmp(Utility::Math::_z_,(sourceFrame->m_rotationAngleAnim[i](Utility::Math::_z_)*(1-interpolate)+targetFrame->m_rotationAngleAnim[i](Utility::Math::_z_)*interpolate));
				rotationAngleAnim.push_back(tmp);
			}
		}
		else{
			// Unimplemented for baked transformations
			transforms = targetFrame->m_transforms;
		}

		joints.clear();
		composeMatrices(translate,scale,rotationAngle,rotationAngleAnim,transforms,skelJoints,joints);
	}

	AnimationFramePtr AnimationClip::getAnimationFrame(float time){
		std::map<float,AnimationFramePtr>::iterator itr;
		if((itr = m_savedFrames.find(time)) != m_savedFrames.end()){
			return itr->second;
		}

		return AnimationFramePtr();
	}

	void AnimationClip::composeMatrices(std::vector<Utility::Math::Vector<float,3>>& translate,
										std::vector<Utility::Math::Vector<float,3>>& scale, 
										std::vector<Utility::Math::Vector<float,3>>& rotationAngle, 
										std::vector<Utility::Math::Vector<float,3>>& rotationAngleAnim,
										std::vector<Utility::Math::Matrix<4,4>>& transforms,
										std::vector<Graphics::SkeletonJointPtr>& skelJoints,
										std::vector<Utility::Math::Matrix<4,4>>& joints){
		Utility::Math::Matrix<4,4> scaleMatrix,translateMatrix,rotationMatrix,rotationMatrixAnim,resultMatrix;

		for(unsigned int i=0; i<skelJoints.size(); i++){
			if(!skelJoints[i]->m_bakedTransform){
				Utility::Math::Quaternion<float> orientX,orientY,orientZ,rotX,rotY,rotZ;

				orientX.createFromAxisAngle(Utility::Math::Vector<>(skelJoints[i]->m_orientAngleX.a),rotationAngle[i](Utility::Math::_x_)*(E_PI/180.0f));
				rotX.createFromAxisAngle(Utility::Math::Vector<>(skelJoints[i]->m_rotateAngleX.a),rotationAngleAnim[i](Utility::Math::_x_)*(E_PI/180.0f));

				orientY.createFromAxisAngle(Utility::Math::Vector<>(skelJoints[i]->m_orientAngleY.a),rotationAngle[i](Utility::Math::_y_)*(E_PI/180.0f));
				rotY.createFromAxisAngle(Utility::Math::Vector<>(skelJoints[i]->m_rotateAngleY.a),rotationAngleAnim[i](Utility::Math::_y_)*(E_PI/180.0f));

				orientZ.createFromAxisAngle(Utility::Math::Vector<>(skelJoints[i]->m_orientAngleZ.a),rotationAngle[i](Utility::Math::_z_)*(E_PI/180.0f));
				rotZ.createFromAxisAngle(Utility::Math::Vector<>(skelJoints[i]->m_rotateAngleZ.a),rotationAngleAnim[i](Utility::Math::_z_)*(E_PI/180.0f));

				rotationMatrix.createFromQuaternion(orientZ*orientY*orientX);
				rotationMatrix.transpose();

				rotationMatrixAnim.createFromQuaternion(rotZ*rotY*rotX);
				rotationMatrixAnim.transpose();
			
				translateMatrix.identity();
				translateMatrix.set(3,0,translate[i](Utility::Math::_x_));
				translateMatrix.set(3,1,translate[i](Utility::Math::_y_));
				translateMatrix.set(3,2,translate[i](Utility::Math::_z_));

				scaleMatrix.identity();
				scaleMatrix.set(0,0,scale[i](Utility::Math::_x_));
				scaleMatrix.set(1,1,scale[i](Utility::Math::_y_));
				scaleMatrix.set(2,2,scale[i](Utility::Math::_z_));
				
				rotationMatrixAnim *= rotationMatrix;
				resultMatrix = scaleMatrix*rotationMatrixAnim*translateMatrix;
			}
			else
				resultMatrix = transforms[i];

			joints.push_back(resultMatrix);
		}

		// 
		for(unsigned int i=0; i<joints.size(); i++){
			for(unsigned int j=0; j<skelJoints[i]->m_children.size(); j++){
				joints[skelJoints[i]->m_children[j]] *= joints[i];
			}
		}
	}

	void AnimationClip::extractSamplerInformation(	float time,
													std::vector<unsigned int>& samplers, 
													std::vector<Utility::Math::Vector<float,3>>& translate, 
													std::vector<Utility::Math::Vector<float,3>>& scale, 
													std::vector<Utility::Math::Vector<float,3>>& rotationAngle, 
													std::vector<Utility::Math::Vector<float,3>>& rotationAngleAnim,
													std::vector<Utility::Math::Matrix<4,4>>& transforms,
													std::vector<Graphics::SkeletonJointPtr>& skelJoints, 
													RenderContext* context){
	
		AnimationSamplerPtr sampler;
		std::vector<float> result;
		unsigned int jointId;

		for(unsigned int i=0; i<skelJoints.size(); i++){
			if(!skelJoints[i]->m_bakedTransform){
				translate.push_back(skelJoints[i]->m_translate);
				scale.push_back(skelJoints[i]->m_scale);

				float orient[3],rotate[3];
				orient[0] = skelJoints[i]->m_orientAngleX(Utility::Math::_w_);
				orient[1] = skelJoints[i]->m_orientAngleY(Utility::Math::_w_);
				orient[2] = skelJoints[i]->m_orientAngleZ(Utility::Math::_w_);

				rotate[0] = skelJoints[i]->m_rotateAngleX(Utility::Math::_w_);
				rotate[1] = skelJoints[i]->m_rotateAngleY(Utility::Math::_w_);
				rotate[2] = skelJoints[i]->m_rotateAngleZ(Utility::Math::_w_);

				rotationAngle.push_back(Utility::Math::Vector<>(orient));
				rotationAngleAnim.push_back(Utility::Math::Vector<>(rotate));
			}
			else
				transforms.push_back(skelJoints[i]->m_transformMatrix);
		}

		for(unsigned int i=0; i<samplers.size(); i++){
			sampler = context->m_animationManager->getAnimationSampler(samplers[i]);
			result = sampler->sample(time);

			jointId = sampler->getTargetJoint();
			if(jointId >= skelJoints.size()){
				LOG_ERROR("Joint id outside joint array");
				assert(0);
				return;
			}

			switch(sampler->getTargetComponent()){
			case AnimationSampler::ATC_TRANSLATION_X :
				translate[jointId](Utility::Math::_x_,result[0]);
				break;
			case AnimationSampler::ATC_TRANSLATION_Y :
				translate[jointId](Utility::Math::_y_,result[0]);
				break;
			case AnimationSampler::ATC_TRANSLATION_Z :
				translate[jointId](Utility::Math::_z_,result[0]);
				break;
			case AnimationSampler::ATC_SCALE_X :
				scale[jointId](Utility::Math::_x_,result[0]);
				break;
			case AnimationSampler::ATC_SCALE_Y :
				scale[jointId](Utility::Math::_y_,result[0]);
				break;
			case AnimationSampler::ATC_SCALE_Z :
				scale[jointId](Utility::Math::_z_,result[0]);
				break;
			case AnimationSampler::ATC_ORIENTATION_X :
				rotationAngleAnim[jointId](Utility::Math::_x_,result[0]);
				break;
			case AnimationSampler::ATC_ORIENTATION_Y :
				rotationAngleAnim[jointId](Utility::Math::_y_,result[0]);
				break;
			case AnimationSampler::ATC_ORIENTATION_Z :
				rotationAngleAnim[jointId](Utility::Math::_z_,result[0]);
				break;
			case AnimationSampler::ATC_VISIBILITY :
				break;
			case AnimationSampler::ATC_TRANSFORM :{
					Utility::Math::Matrix<4,4> matrix;
					std::copy(&result[0],&result[0]+16,matrix.m_data);
					matrix.transpose();

					transforms[jointId] = matrix;
					break;
				}
			default:
				break;
			}
		}
	}
}